#ifndef __KINEMATICS_H__
#define __KINEMATICS_H__

#include <Arduino.h>

/**
 * @brief 存储电机参数
 */
typedef struct
{
    float per_pulse_distance;  // 单个脉冲对应轮子前进的距离
    int16_t motor_speed;       // 当前电机速度mm/s,计算时使用
    int64_t last_encoder_tick; // 上次电机的编码器读数
} motor_param_t;

/**
 * @brief 处理机器人运动学
 */
class Kinematics
{
private:
    motor_param_t motor_param_[2]; // 存储两个电机的参数
    uint64_t last_update_time_;     // 上次更新数据的时间，单位ms
    float wheel_distance_;         // 轮子间距
public:
    Kinematics() = default;
    ~Kinematics() = default;
    /**
     * @brief 设置电机参数，包括编号和每个脉冲对应的轮子前进距离
     *
     * @param id 电机标识
     * @param per_pulse_distance 每个脉冲对应的轮子前进距离
     */
    void set_motor_param(uint8_t id, float per_pulse_distance);
    /**
     * @brief 设置轮子间距
     *
     * @param wheel_distance 轮子间距
     */
    void set_wheel_distance(float wheel_distance);
    /**
     * @brief 逆运动学计算，将线速度和角速度转换为左右轮的速度
     *
     * @param linear_speed 线速度（单位：mm/s）
     * @param angle_speed 角速度（单位：rad/s）
     * @param[out] out_left_speed 左轮速度（单位：mm/s）
     * @param[out] out_right_speed 右轮速度（单位：mm/s）
     */
    void kinematic_inverse(float linear_speed, float angle_speed, float &out_left_speed, float &out_right_speed);
    /**
     * @brief 正运动学计算，将左右轮的速度转换为线速度和角速度
     *
     * @param left_speed 左轮速度（单位：mm/s）
     * @param right_speed 右轮速度（单位：mm/s）
     * @param[out] out_linear_speed 线速度（单位：mm/s）
     * @param[out] out_angle_speed 角速度（单位：rad/s）
     */
    void kinematic_forward(float left_speed, float right_speed, float &out_linear_speed, float &out_angle_speed);
    /**
     * @brief 更新电机速度和编码器数据
     */
    void update_motor_speed(uint64_t current_time, int32_t left_tick, int32_t right_tick);
    /**
     * @brief 获取电机速度
     *
     * @param id 电机标识
     * @return int16_t 电机速度（单位：mm/s）
     */
    int16_t get_motor_speed(uint8_t id);
};

#endif